'''
Created on 16 mars 2012

@author: menez
'''

import sys
import pygame
import pyroomba

port = "/dev/tty.usbserial-A900fM5C"
print port

robot = pyroomba.Roomba(port)

robot.start()
robot.safe()
robot.stream_samples(pyroomba.sensors.LEFT_ENCODER, pyroomba.sensors.RIGHT_ENCODER,
                      pyroomba.sensors.DISTANCE, pyroomba.sensors.ANGLE)


motors = [0, 0]

def set_motor(motor, value):
    global motors
    motors[motor] = value

pygame.init()
screen = pygame.display.set_mode((640, 480))
 
done=False
while done==False:
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
            done=True
 
    # User pressed down on a key
        if event.type == pygame.KEYDOWN:
            # Figure out if it was an arrow key. If so
            # adjust speed.
            print "Keydown"
            if event.key == pygame.K_LEFT:
                robot.motors(vacuum=True)
            if event.key == pygame.K_RIGHT:
                robot.motors(vacuum=False)
            if event.key == pygame.K_UP:
                set_motor(1,-100) 
            if event.key == pygame.K_DOWN:
                set_motor(0,-100) 
        """         
        # User let up on a key
        if event.type == pygame.KEYUP:
            print "Keyup"
            # If it is an arrow key, reset vector back to zero
            if event.key == pygame.K_LEFT:
                x_speed=0
            if event.key == pygame.K_RIGHT:
                x_speed=0
            if event.key == pygame.K_UP:
                y_speed=0
            if event.key == pygame.K_DOWN:
                y_speed=0
        """    
 
    robot.drive_pwm(*motors)
    
    try:
        print robot.poll()
    except:
        print "Error"
    
